import rospy
from sensor_msgs.msg import Joy
from rovmsg.msg import rovmsg
import rostopic
import roslib
import serial
    # TODO:
        # 写一个串口通信，与stm32建立联系即可


class Base:
    def __init__(self):

        rospy.init_node("rov_base")
        topic_type, real_topic, fields = rostopic.get_topic_type("/ROV/cmd_vel")        
        data_class = roslib.message.get_message_class(topic_type)
        rospy.Subscriber(real_topic, data_class, self.call_back)

        rospy.spin()

    def verify(data1, *data):
        check = data1
        for var in data:
            check = check + var
        check = (check+1105) % 128
        return check
    
    def call_back(self,data:rovmsg):
        
        pass

    def sendMsg(upDown, speedUpDown, forwardBack, speedForwardBack, direction, speedDirection, shift, shiftSpeed, POS, PIT, SpeedPIT, ROL, SpeedROL, Light, LED):
        # ser.write("0x15\r\n".encode("utf-8"))
        # check_stm32 = verify(upDown, speedUpDown, forwardBack, speedForwardBack, direction, speedDirection, shift, shiftSpeed, POS, PIT, SpeedPIT, ROL, SpeedROL, Light, LED)
        # msg = "AA"+chr(upDown)+chr(speedUpDown)+chr(forwardBack)+chr(speedForwardBack)+chr(direction)+chr(speedDirection)+chr(shift)+chr(shiftSpeed) +\
        #     chr(POS)+chr(PIT)+chr(SpeedPIT)+chr(ROL)+chr(SpeedROL)+chr(Light)+chr(LED)+chr(check_stm32)+"BByz\r\n"
        # check_stm32 = 0
        # ser.write(msg.encode())  # se
        # ser.flushInput()  #
        pass

if __name__ == "__main__":
    controller = Base()